package com.hidquat;

/**
 * Created on 2017/6/10.
 */

public class Quaternion {
    public double q0;
    public double q1;
    public double q2;
    public double q3;

    public Quaternion(Quaternion q){
        this.q0 = q.q0;
        this.q1 = q.q1;
        this.q2 = q.q2;
        this.q3 = q.q3;
    }

   public Quaternion(double q0, double q1, double q2, double q3){
        this.q0 = q0;
        this.q1 = q1;
        this.q2 = q2;
        this.q3 = q3;
    }

    public Quaternion(double x, double y, double z){
        double c1 = Math.cos(y);
        double s1 = Math.sin(y);
        double c2 = Math.cos(z);
        double s2 = Math.sin(z);
        double c3 = Math.cos(x);
        double s3 = Math.sin(x);
        q0 = Math.sqrt(1.0f + c1 * c2 + c1 * c3 - s1 * s2 * s3 + c2*c3) / 2;
        q1 = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / (4.0f * q0);
        q2 = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / (4.0f * q0);
        q3 = (-s1 * s3 + c1 * s2 * c3 + s2) /(4.0f * q0);
    }

    public void multi(Quaternion a) {
        Quaternion tmp = new Quaternion(q0, q1, q2, q3);

        this.q0 = a.q0*tmp.q0 - a.q1*tmp.q1 - a.q2*tmp.q2 - a.q3*tmp.q3;
        this.q1 = a.q1*tmp.q0 + a.q0*tmp.q1 - a.q3*tmp.q2 + a.q2*tmp.q3;
        this.q2 = a.q2*tmp.q0 + a.q3*tmp.q1 + a.q0*tmp.q2 - a.q1*tmp.q3;
        this.q3 = a.q3*tmp.q0 - a.q2*tmp.q1 + a.q1*tmp.q2 + a.q0*tmp.q3;
    }

    public void refresh(double q0, double q1, double q2, double q3){
        this.q0 = q0;
        this.q1 = q1;
        this.q2 = q2;
        this.q3 = q3;
    }

    /* normalization of quaternion */
    public void normal(){
        double model = Math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3 );
        q0 /= model;
        q1 /= model;
        q2 /= model;
        q3 /= model;
    }

    public void getEulerDegree(double euler[]) {
        euler[0] = Math.toDegrees( Math.atan2( 2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2) ) );
        euler[1] = Math.toDegrees( Math.asin( 2*(q0*q2 - q1*q3) ) );
        euler[2] = Math.toDegrees( Math.atan2( 2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3) ) );
    }

    public void getEulerRadians(double euler[]) {
        euler[0] = Math.atan2( 2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2) );
        euler[1] = Math.asin( 2*(q0*q2 - q1*q3) );
        euler[2] = Math.atan2( 2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3) );
    }

    public void transformVectorBodyToEarth(double []v) {
        double q0q0 = q0 * q0;
        double q0q1 = q0 * q1;
        double q0q2 = q0 * q2;
        double q0q3 = q0 * q3;
        double q1q1 = q1 * q1;
        double q1q2 = q1 * q2;
        double q1q3 = q1 * q3;
        double q2q2 = q2 * q2;
        double q2q3 = q2 * q3;
        double q3q3 = q3 * q3;

        double x = (q0q0+q1q1-q2q2-q3q3)*v[0] + 2*(q1q2-q0q3)*v[1] + 2*(q1q3+q0q2)*v[2];
        double y = 2*(q1q2+q0q3)*v[0] + (q0q0-q1q1+q2q2-q3q3)*v[1] + 2*(q2q3-q0q1)*v[2];
        double z = 2*(q1q3-q0q2)*v[0] + 2*(q2q3+q0q1)*v[1] +(q0q0-q1q1-q2q2+q3q3)*v[2];

        v[0] = x;
        v[1] = y;
        v[2] = z;
    }

    public void transformVectorEarthToBody(double []v) {
        double q0q0 = q0 * q0;
        double q0q1 = q0 * q1;
        double q0q2 = q0 * q2;
        double q0q3 = q0 * q3;
        double q1q1 = q1 * q1;
        double q1q2 = q1 * q2;
        double q1q3 = q1 * q3;
        double q2q2 = q2 * q2;
        double q2q3 = q2 * q3;
        double q3q3 = q3 * q3;

        double x = (q0q0+q1q1-q2q2-q3q3)*v[0] + 2*(q1q2+q0q3)*v[1] + 2*(q1q3-q0q2)*v[2];
        double y = 2*(q1q2-q0q3)*v[0] + (q0q0-q1q1+q2q2-q3q3)*v[1] + 2*(q2q3+q0q1)*v[2];
        double z = 2*(q1q3+q0q2)*v[0] + 2*(q2q3-q0q1)*v[1] +(q0q0-q1q1-q2q2+q3q3)*v[2];

        v[0] = x;
        v[1] = y;
        v[2] = z;
    }
}
